% outputs from humanData
% get average mass
% add path

addpath_output()
% name list 9 person
name  = {'Charles','Fred','Martin','Ram','Selina',... % 1st experiment
    'lily','po','ryan','vic'};  % 2nd experiment

% CONSTANT
% the number of subjects from the 1st experiment
exp1st = 5;
% the index of subjects use the right leg as swing leg during the step
rIndex = [6,7,9];
% experimental sampling frequency
fs = 480;
model = 'NAO';
% model = 'AMBER';

time_rec = zeros(9,1);
for index = 1:length(name) % 5
  
    % get human data, with breaking pts for each step
    humanData = getData(name{1,index});
    
%     sum_weight  = sum_weight+humanData.weight; % for average weight
    % breaking pts for each step
    StartPt = humanData.sns_brk(2);
    EndPt = humanData.sns_brk(3);
    
    % position
    if index <= exp1st
        [xpos,ypos,zpos] = getPosCartesian(humanData); % subjects from experiment 1st
    else
        [xpos,ypos,zpos] = getPosCartesianNew(humanData); % subjects from experiment 2st
    end

    %%%%%%%%%%%  time  %%%%%%%%%%%%%%
    % scale time
    % from [c:d]
    ns_time_pre = (0:length(StartPt:EndPt)-1)'/fs;
    ns_ret = ns_time_pre;
    xpara_time = 1/ns_ret(end); % simply sacled time to 1
    ns_time = ns_ret*xpara_time;
    %%%% record time %%%%%%%%
    time_rec(index) = ns_ret(end);
    
   
    
    %%%%%%%%%%%% calculate human joint angles %%%%%%%%%%%%%%%%%
    if ismember(index,rIndex)
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,2);
    else
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,1);
    end
    
    %%%%%%%%%%% use joint angles to calculate the outputs %%%%%%%%%%
     q = [angle.sa;angle.sknee;angle.theta3;angle.theta4;angle.nsknee];
     
%      outputs = cal_robot_para(q,model);
     outputs = cal_robot_para_18(q,model);
     
%      [all_outputs,ave_time] = cal_beam_data(index, ns_time, outputs);
     %%%%%%%%%%%%%%%%
         % 1. hip position
    [ave_dataHP,ave_timeHP] = BeamData(ns_time',outputs.hippos);
    all_outputs. ave_dataHP_s(index,:) =ave_dataHP;
%     outputs.delta_hippos(1)
%     plot(ave_dataHP); hold on;


    % 2. linearized hip position
    [ave_dataHPL,ave_timeHPL] = BeamData(ns_time,outputs.delta_hippos);
    all_outputs.ave_dataHPL_s(index,:) =ave_dataHPL;
%     plot(ave_dataHPL); hold on;


    % 3. non-stance slope
    [ave_dataNSslope,ave_timeNSslope] = BeamData(ns_time,outputs.nsslope);
    all_outputs.ave_dataNSslope_s(index,:) =ave_dataNSslope;
%     plot(ave_dataNSslope); hold on;

    % 4. linearized non-stance slope
    [ave_dataNSslopeL,ave_timeNSslopeL] = BeamData(ns_time,outputs.delta_nsslope);
    all_outputs.ave_dataNSslopeL_s(index,:) =ave_dataNSslopeL; 
%     plot(ave_dataNSslopeL); hold on;
%     5. stance COM slope
[ave_dataSCOM,ave_timeSCOM] = BeamData(ns_time, outputs.stCOM);
    all_outputs.ave_dataSCOM_s(index,:) = ave_dataSCOM;
        
%     6. linearized stance COM slope
[ave_dataSCOML,ave_timeSCOML] = BeamData(ns_time, outputs.delta_stCOM);
    all_outputs.ave_dataSCOML_s(index,:) = ave_dataSCOML;
    
% % %     % 5. stance torso slope
% % %     [ave_dataST,ave_timeST] = BeamData(ns_time, outputs.storso);
% % %     all_outputs.ave_dataST_s(index,:) = ave_dataST;
% % % %     plot(ave_dataST,'r'); hold on;
% % %     
% % %     % 6. Linearized stance torso slope
% % %     [ave_dataSTL,ave_timeSTL] = BeamData(ns_time,outputs.delta_storso);
% % %     all_outputs.ave_dataSTL_s(index,:) = ave_dataSTL;
% % % %     plot(ave_dataSTL); hold on;
% % %     
% % %     
% % %     % 7. non-stance torso slope
% % %     [ave_dataNST,ave_timeNST] = BeamData(ns_time,outputs.nstorso);
% % %     all_outputs.ave_dataNST_s(index,:) = ave_dataNST;
% % %     
% % %     
% % %     % 8. linearized non-stance torso slope
% % %     [ave_dataNSTL,ave_timeNSTL] = BeamData(ns_time,outputs.delta_nstorso);
% % %     all_outputs.ave_dataNSTL_s(index,:) = ave_dataNSTL;
% % %     
% % %     
% % %     % 9. COM
% % %     [ave_dataCOM,ave_timeCOM] = BeamData(ns_time,outputs.com);
% % %     all_outputs.ave_dataCOM_s(index,:) = ave_dataCOM;
% % %     
% % %     
% % %     % 10. linearized COM
% % %     [ave_dataCOML,ave_timeCOML] = BeamData(ns_time,outputs.delta_com);
% % %     all_outputs.ave_dataCOML_s(index,:) = ave_dataCOML;
    
    ave_time = ave_timeHP;
     %%%%%%%%%%%%%%%%
     
     
     %%%%
%      all_outputs.ave_dataHP_s(index,:) =ave_dataHP;
%      ave_dataHPL_s(index,:) =ave_dataHPL;
%      ret.ave_dataNSslope_s(index,:) =ave_dataNSslope;
     %%%
end
%%

% calculate the mean data
 [Xmean,XupperB,XlowerB] = cal_mean_outputs(all_outputs);
%% save the data
 ns_time_mean = mean(time_rec)*ave_time'; % averaged time
 data_name = 'mean_data';
 folder_name = ['data_' model '_18'];
 save_outputs(Xmean,ns_time_mean,data_name,folder_name);
